

#include "RWPiooneerDistSensors.h"

using namespace std;
using namespace webots;


#define MAX_SENSOR_NUMBER 16
#define RANGE (1024 / 2)
#define BOUND(x, a, b) (((x)<(a))?(a):((x)>(b))?(b):(x))




RWPioDistSens::RWPioDistSens()
{


}
RWPioDistSens::~RWPioDistSens()
{

}

void RWPioDistSens::init(webots::DifferentialWheels* DFin, ros::NodeHandle* n_in,std::string name)
{
  DF = DFin;
  n = n_in;
  //Initialization of parameters for pioneer robot
  //Max speed and sensor matrix for computation of wheel speed according to braitenberg controller
  pioneer2_max_speed = 100.0;
  //pioneer2_matrix = {{-1, 15}, {-3, 13}, {-3, 8}, {-2, 7}, {-3, -4}, {-4, -2}, {-3, -2}, {-1, -1}, {-1, -1}, {-2, -3}, {-2, -4}, {4, -3}, {7, -5}, {7, -3},{10, -2}, {11, -1} };
  double pi_mat1[] = {-1,-3,-3,-2,-3,-4-3,-1,-1,-2,-2,4,7,7,10,11};
  double pi_mat2[] = {15,13,8,7,-4,-2,-2,-1,-1,-3,-2,4,-3,-5,-3,-2, -1};
  time_step = DF->getBasicTimeStep();

  stringstream ss;
  for (int i = 0;i<=15;i++)
  {
	pioneer2_matrix[i][0] = pi_mat1[i];
	pioneer2_matrix[i][1] = pi_mat2[i];
    ss.flush();
    ss<<"ds"<<i;
    DSENS[i] = DF->getDistanceSensor(ss.str());
    DSENS[i]->enable(time_step);
  }
}


void RWPioDistSens::iterate()
{
  ros::Time now = ros::Time::now();
  //DISTANCE SENSORS
  for (int i = 0;i<=15;i++)
  {
    DSENS_VAL[i] = DSENS[i]->getValue();
  }

  double speed[2];
  for (int i = 0; i < 2; i++) {
    speed[i] = 0.0;
    for (int j = 0; j <=15; j++)
    {
      speed[i] += pioneer2_matrix[j][i] * (1.0 - (DSENS_VAL[j] / RANGE));
    }
    speed[i] = BOUND(speed[i], -pioneer2_max_speed, pioneer2_max_speed);
  }
  DF->setSpeed(speed[0], speed[1]);
}



